// Deng's FOC AS5600测试例程 测试硬件：灯哥开源FOC V3.0

#include <SimpleFOC.h>

MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);
TwoWire I2Ctwo = TwoWire(1);
uint32_t frequency = 400000;

void setup()
{

    Serial.begin(115200);
    _delay(750);

    I2Cone.begin(5, 18, frequency); // SDA1,SCL1
    I2Ctwo.begin(21, 22, frequency);
    sensor0.init(&I2Cone);
    sensor1.init(&I2Ctwo);
}

void loop()
{
    sensor0.update();
    sensor1.update();
    //_delay(200);
    Serial.print(sensor0.getAngle());
    Serial.print(" - ");
    Serial.print(sensor1.getAngle());
    Serial.println();
}
